using System;
using System.Collections.Generic;
using LegoBotPrimaAbstractions;

namespace LegoBotPrima.Actions
{
    public class GoToAngleBotAction : BotAction
    {
        private static readonly object _synch_OnCompassChanged = new object();
        private readonly LinkedList<SensorData> _angles = new LinkedList<SensorData>();
        private Dir _direction;

        public double Angle { get; set; }

        public override void Initialize()
        {
            var diff = MathUtil.AnglesDiff(Angle, Workflow.RotationAngle);
            this._direction = DefineDirection(diff);
            switch (this._direction)
            {
                case Dir.Left:
                    Workflow.GoLeftSlow();
                    break;
                case Dir.Right:
                    Workflow.GoRightSlow();
                    break;
                case Dir.Stop:
                    this.End();
                    break;
            }
        }

        public override void OnCompassChanged(double degrees, System.DateTime timeStamp)
        {
            if(!IsRunning) return;

            lock (_synch_OnCompassChanged)
            {
                this._angles.AddLast(new SensorData{ Angle = degrees, TimeStamp = timeStamp });

                var diff = MathUtil.AnglesDiff(Angle, degrees);
                if(Math.Abs(diff) < 15)
                {
                    this.End();
                    return;
                }
                var dir = DefineDirection(diff);
                if (dir == Dir.Stop || this._direction != dir)
                {
                    this.End();
                }
            }
        }

        

        public override string ToString()
        {
            return base.ToString() + " Angle: " + Angle + " RequestedNextAction:" + RequestedNextAction;
        }



        #region Private Methods
        private void End()
        {
            this.RequestedNextAction = new ForwardBotAction();
            IsRunning = false;
            Workflow.Stop();
            Workflow.EndAction(this);
        }
        private static Dir DefineDirection(double diff)
        {
            return diff > 0 ? Dir.Left : diff < 0 ? Dir.Right : Dir.Stop;
        }

        
        #endregion

        enum Dir
        {
            Left,
            Right,
            Stop
        } 
    }
}